#include "PIC1_main.h"
#pragma code

BuildingFrame buildingFrameGyro;

BuildingFrame buildingFrameAcc;

UINT32 TempsPIC1 = 0;

static UINT8 cpt10ms = 0;

volatile INT16 gyro_x;
volatile INT16 gyro_y;
volatile INT16 gyro_z;

UINT16 acc_x;
UINT16 acc_y;
UINT16 acc_z;

UINT8 gyro = 0;
UINT8 acc = 0;

// High priority interrupt vector
#pragma code InterruptVectorHigh = 0x08
void InterruptVectorHigh (void)
{
	_asm
	goto InterruptServiceHigh //jump to interrupt routine
	_endasm
}

// Low priority interrupt vector
#pragma code InterruptVectorLow = 0x18
void InterruptVectorLow (void)
{
	_asm
	goto InterruptServiceLow //jump to interrupt routine
	_endasm
}

// High priority interrupt routine

#pragma interrupt InterruptServiceHigh
void InterruptServiceHigh(void)
{	
	//Interruption GPS
	if(PIR1bits.RC1IF) //IT sur GPS
	{
		UINT8 tamp;
		LED_GPSL70=1;
		tamp = getc1USART();
		parser_gps(tamp, TempsPIC1);
		LED_GPSL70=0;
	}else 
	//Interruption TIMER
	if(PIR1bits.TMR2IF){	//IT sur TIMER 2
		cpt10ms++;
		if(cpt10ms==10){
			//LED_TEST=~LED_TEST;
			TempsPIC1++;
			cpt10ms = 0;
		}
		PIR1bits.TMR2IF = 0;
	}else 
	//Interruption Gyrometre
	if(INTCONbits.INT0IF){
		LED_GYRO=1;
		gyro_x=Get_Gyro_X();
		gyro_y=Get_Gyro_Y();
		gyro_z=Get_Gyro_Z();
		if(gyro==0){
			buildGyroAndAcceleroFrame(gyro_x,gyro_y,gyro_z,TRUE,&buildingFrameGyro,TempsPIC1);
			gyro++;
		}else if(gyro==2){
			gyro = 0;
		}else{
			gyro++;
		}
		LED_GYRO=0;
		INTCONbits.INT0IF = 0;
	}
}

#pragma interruptlow InterruptServiceLow
void InterruptServiceLow(void)
{
	//Interruption Accelerometre
	if(INTCON3bits.INT1IF) //IT sur ACC
	{
		LED_ACC=1;
		ACC_get_xyz(&acc_x, &acc_y, &acc_z);//necessaire pour acquiter l'int cote accelero
		if(acc==0){
			INTCONbits.GIEH = 0;
			buildGyroAndAcceleroFrame(acc_x,acc_y,acc_z,FALSE,&buildingFrameAcc,TempsPIC1);
			INTCONbits.GIEH = 1;    
			acc++;
		}else{
			acc = 0;
		}
		LED_ACC=0;
		INTCON3bits.INT1IF = 0;
	}
}

void main(void)
{
	UINT8 i;
	BUFFER_init();
	BUFFERGPS_init();
	initPic();
	for(i=0;i<10;i++) Delay10KTCYx(0);
	init_gps_l70();
	init_acc();
	gyro_init();
	initFrame();
	while(1){
		BUFFER_send();	
	}
}



